home *** CD-ROM | disk | FTP | other *** search
/ Amiga Plus 2002 #11 / Amiga Plus CD - 2002 - No. 11.iso / Tools / Development / TinyGL / ami / content / ad709 / tinygl / src / zmath.c < prev    next >
Encoding:
C/C++ Source or Header  |  2002-08-15  |  6.2 KB  |  332 lines

  1. /*$T zmath.c GC 1.137 08/09/02 17:47:17 */
  2.  
  3. /*
  4.  * Some simple mathematical functions. Don't look for some logic in the function
  5.  * names :-)
  6.  */
  7. #include <stdio.h>
  8. #include <stdlib.h>
  9. #include <string.h>
  10. #include <math.h>
  11. #include "zmath.h"
  12.  
  13. /* Gestion des matrices 4x4 */
  14. void gl_M4_Id(M4 *a) {
  15.     a->m[0][0] = 1;
  16.     a->m[0][1] = 0;
  17.     a->m[0][2] = 0;
  18.     a->m[0][3] = 0;
  19.     a->m[1][0] = 0;
  20.     a->m[1][1] = 1;
  21.     a->m[1][2] = 0;
  22.     a->m[1][3] = 0;
  23.     a->m[2][0] = 0;
  24.     a->m[2][1] = 0;
  25.     a->m[2][2] = 1;
  26.     a->m[2][3] = 0;
  27.     a->m[3][0] = 0;
  28.     a->m[3][1] = 0;
  29.     a->m[3][2] = 0;
  30.     a->m[3][3] = 1;
  31. }
  32.  
  33. /* */
  34. int gl_M4_IsId(M4 *a) {
  35.     int i, j;
  36.     /*~~~~~*/
  37.  
  38.     for(i = 0; i < 4; i++) {
  39.         for(j = 0; j < 4; j++) {
  40.             if(i == j) {
  41.                 if(a->m[i][j] != 1.0) {
  42.                     return 0;
  43.                 }
  44.             }
  45.             else if(a->m[i][j] != 0.0) {
  46.                 return 0;
  47.             }
  48.         }
  49.     }
  50.  
  51.     return 1;
  52. }
  53.  
  54. /* */
  55. void gl_M4_Mul(M4 *c, M4 *a, M4 *b) {
  56.     int        i, j, k;
  57.     float    s;
  58.     for(i = 0; i < 4; i++) {
  59.         for(j = 0; j < 4; j++) {
  60.             s = 0.0;
  61.             for(k = 0; k < 4; k++) {
  62.                 s += a->m[i][k] * b->m[k][j];
  63.             }
  64.  
  65.             c->m[i][j] = s;
  66.         }
  67.     }
  68. }
  69.  
  70. /* c=c*a */
  71. void gl_M4_MulLeft(M4 *c, M4 *b) {
  72.     int        i, j, k;
  73.     float    s;
  74.     M4        a;
  75.  
  76.     /* memcpy(&a, c, 16*sizeof(float)); */
  77.     a = *c;
  78.  
  79.     for(i = 0; i < 4; i++) {
  80.         for(j = 0; j < 4; j++) {
  81.             s = 0.0;
  82.             for(k = 0; k < 4; k++) {
  83.                 s += a.m[i][k] * b->m[k][j];
  84.             }
  85.  
  86.             c->m[i][j] = s;
  87.         }
  88.     }
  89. }
  90.  
  91. /* */
  92. void gl_M4_Move(M4 *a, M4 *b) {
  93.     memcpy(a, b, sizeof(M4));
  94. }
  95.  
  96. /* */
  97. void gl_MoveV3(V3 *a, V3 *b) {
  98.     memcpy(a, b, sizeof(V3));
  99. }
  100.  
  101. /* */
  102. void gl_MulM4V3(V3 *a, M4 *b, V3 *c) {
  103.     a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z + b->m[0][3];
  104.     a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z + b->m[1][3];
  105.     a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z + b->m[2][3];
  106. }
  107.  
  108. /* */
  109. void gl_MulM3V3(V3 *a, M4 *b, V3 *c) {
  110.     a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z;
  111.     a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z;
  112.     a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z;
  113. }
  114.  
  115. /* */
  116. void gl_M4_MulV4(V4 *a, M4 *b, V4 *c) {
  117.     a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z + b->m[0][3] * c->W;
  118.     a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z + b->m[1][3] * c->W;
  119.     a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z + b->m[2][3] * c->W;
  120.     a->W = b->m[3][0] * c->X + b->m[3][1] * c->Y + b->m[3][2] * c->Z + b->m[3][3] * c->W;
  121. }
  122.  
  123. /* transposition of a 4x4 matrix */
  124. void gl_M4_Transpose(M4 *a, M4 *b) {
  125.     a->m[0][0] = b->m[0][0];
  126.     a->m[0][1] = b->m[1][0];
  127.     a->m[0][2] = b->m[2][0];
  128.     a->m[0][3] = b->m[3][0];
  129.  
  130.     a->m[1][0] = b->m[0][1];
  131.     a->m[1][1] = b->m[1][1];
  132.     a->m[1][2] = b->m[2][1];
  133.     a->m[1][3] = b->m[3][1];
  134.  
  135.     a->m[2][0] = b->m[0][2];
  136.     a->m[2][1] = b->m[1][2];
  137.     a->m[2][2] = b->m[2][2];
  138.     a->m[2][3] = b->m[3][2];
  139.  
  140.     a->m[3][0] = b->m[0][3];
  141.     a->m[3][1] = b->m[1][3];
  142.     a->m[3][2] = b->m[2][3];
  143.     a->m[3][3] = b->m[3][3];
  144. }
  145.  
  146. /* inversion of an orthogonal matrix of type Y=M.X+P */
  147. void gl_M4_InvOrtho(M4 *a, M4 b) {
  148.     int        i, j;
  149.     float    s;
  150.     for(i = 0; i < 3; i++) {
  151.         for(j = 0; j < 3; j++) {
  152.             a->m[i][j] = b.m[j][i];
  153.         }
  154.  
  155.         a->m[3][0] = 0.0;
  156.         a->m[3][1] = 0.0;
  157.         a->m[3][2] = 0.0;
  158.         a->m[3][3] = 1.0;
  159.         for(i = 0; i < 3; i++) {
  160.             s = 0;
  161.             for(j = 0; j < 3; j++) {
  162.                 s -= b.m[j][i] * b.m[j][3];
  163.             }
  164.  
  165.             a->m[i][3] = s;
  166.         }
  167.     }
  168. }
  169.  
  170. /* Inversion of a general nxn matrix. Note:: m is destroyed */
  171. int Matrix_Inv(float *r, float *m, int n) {
  172.     int        i, j, k, l;
  173.     float    max, tmp, t;
  174.  
  175.     /* identitée dans r */
  176.     for(i = 0; i < n * n; i++) {
  177.         r[i] = 0;
  178.     }
  179.  
  180.     for(i = 0; i < n; i++) {
  181.         r[i * n + i] = 1;
  182.     }
  183.  
  184.     for(j = 0; j < n; j++) {
  185.         /* recherche du nombre de plus grand module sur la colonne j */
  186.         max = m[j * n + j];
  187.         k = j;
  188.         for(i = j + 1; i < n; i++) {
  189.             if(fabs(m[i * n + j]) > fabs(max)) {
  190.                 k = i;
  191.                 max = m[i * n + j];
  192.             }
  193.         }
  194.  
  195.         /* non intersible matrix */
  196.         if(max == 0) {
  197.             return 1;
  198.         }
  199.  
  200.         /* permutation des lignes j et k */
  201.         if(k != j) {
  202.             for(i = 0; i < n; i++) {
  203.                 tmp = m[j * n + i];
  204.                 m[j * n + i] = m[k * n + i];
  205.                 m[k * n + i] = tmp;
  206.  
  207.                 tmp = r[j * n + i];
  208.                 r[j * n + i] = r[k * n + i];
  209.                 r[k * n + i] = tmp;
  210.             }
  211.         }
  212.  
  213.         /* multiplication de la ligne j par 1/max */
  214.         max = 1 / max;
  215.         for(i = 0; i < n; i++) {
  216.             m[j * n + i] *= max;
  217.             r[j * n + i] *= max;
  218.         }
  219.  
  220.         for(l = 0; l < n; l++) {
  221.             if(l != j) {
  222.                 t = m[l * n + j];
  223.                 for(i = 0; i < n; i++) {
  224.                     m[l * n + i] -= m[j * n + i] * t;
  225.                     r[l * n + i] -= r[j * n + i] * t;
  226.                 }
  227.             }
  228.         }
  229.     }
  230.  
  231.     return 0;
  232. }
  233.  
  234. /* inversion of a 4x4 matrix */
  235. void gl_M4_Inv(M4 *a, M4 *b) {
  236.     M4    tmp;
  237.     memcpy(&tmp, b, 16 * sizeof(float));
  238.  
  239.     /* tmp=*b; */
  240.     Matrix_Inv(&a->m[0][0], &tmp.m[0][0], 4);
  241. }
  242.  
  243. /* */
  244. void gl_M4_Rotate(M4 *a, float t, int u) {
  245.     float    s, c;
  246.     int        v, w;
  247.     if((v = u + 1) > 2) {
  248.         v = 0;
  249.     }
  250.  
  251.     if((w = v + 1) > 2) {
  252.         w = 0;
  253.     }
  254.  
  255.     s = sin(t);
  256.     c = cos(t);
  257.     gl_M4_Id(a);
  258.     a->m[v][v] = c;
  259.     a->m[v][w] = -s;
  260.     a->m[w][v] = s;
  261.     a->m[w][w] = c;
  262. }
  263.  
  264. /* inverse of a 3x3 matrix */
  265. void gl_M3_Inv(M3 *a, M3 *m) {
  266.     float    det;
  267.  
  268.     det = m->m[0][0] *
  269.         m->m[1][1] *
  270.         m->m[2][2] -
  271.         m->m[0][0] *
  272.         m->m[1][2] *
  273.         m->m[2][1] -
  274.         m->m[1][0] *
  275.         m->m[0][1] *
  276.         m->m[2][2] +
  277.         m->m[1][0] *
  278.         m->m[0][2] *
  279.         m->m[2][1] +
  280.         m->m[2][0] *
  281.         m->m[0][1] *
  282.         m->m[1][2] -
  283.         m->m[2][0] *
  284.         m->m[0][2] *
  285.         m->m[1][1];
  286.  
  287.     a->m[0][0] = (m->m[1][1] * m->m[2][2] - m->m[1][2] * m->m[2][1]) / det;
  288.     a->m[0][1] = -(m->m[0][1] * m->m[2][2] - m->m[0][2] * m->m[2][1]) / det;
  289.     a->m[0][2] = -(-m->m[0][1] * m->m[1][2] + m->m[0][2] * m->m[1][1]) / det;
  290.  
  291.     a->m[1][0] = -(m->m[1][0] * m->m[2][2] - m->m[1][2] * m->m[2][0]) / det;
  292.     a->m[1][1] = (m->m[0][0] * m->m[2][2] - m->m[0][2] * m->m[2][0]) / det;
  293.     a->m[1][2] = -(m->m[0][0] * m->m[1][2] - m->m[0][2] * m->m[1][0]) / det;
  294.  
  295.     a->m[2][0] = (m->m[1][0] * m->m[2][1] - m->m[1][1] * m->m[2][0]) / det;
  296.     a->m[2][1] = -(m->m[0][0] * m->m[2][1] - m->m[0][1] * m->m[2][0]) / det;
  297.     a->m[2][2] = (m->m[0][0] * m->m[1][1] - m->m[0][1] * m->m[1][0]) / det;
  298. }
  299.  
  300. /* vector arithmetic */
  301. int gl_V3_Norm(V3 *a) {
  302.     float    n;
  303.     n = sqrt(a->X * a->X + a->Y * a->Y + a->Z * a->Z);
  304.     if(n == 0) {
  305.         return 1;
  306.     }
  307.  
  308.     a->X /= n;
  309.     a->Y /= n;
  310.     a->Z /= n;
  311.     return 0;
  312. }
  313.  
  314. /* */
  315. V3 gl_V3_New(float x, float y, float z) {
  316.     V3    a;
  317.     a.X = x;
  318.     a.Y = y;
  319.     a.Z = z;
  320.     return a;
  321. }
  322.  
  323. /* */
  324. V4 gl_V4_New(float x, float y, float z, float w) {
  325.     V4    a;
  326.     a.X = x;
  327.     a.Y = y;
  328.     a.Z = z;
  329.     a.W = w;
  330.     return a;
  331. }
  332.